//These are all the utility/building block functions used in autonomous

// ------------------------------ Arm ---------------------------------------------
void changeArmStateW(int state) { // Changes arm state, then waits until complete.
  armState = state;
  wait1Msec(100);
  int waitTime = 0;
  while (armAction == 1 && waitTime < 150) {
    wait1Msec(10);
    waitTime += 1;
  }
}

void scoreArm(int state) { // Changes arm state, then waits until complete.
  armState = state;
  wait1Msec(100);
  int waitTime = 0;
  while (armAction == 1 && waitTime < 120) {
    wait1Msec(10);
    wiggleon = true;
    waitTime += 1;
  }
  wiggleon = false;
  wheelLeftPower = 0;
  wheelRightPower = 0;
}

// ---------------------------------- Turn ---------------------------------------------
void turnTP(float ticks) // Turns robot about centre depending on ticks with P-controller (no D-controller)
{
  int prevEncL; int prevEncR;
	SensorValue[LeftEncoder] = 0;
	SensorValue[RightEncoder] = 0;
	int wheelLNormal = minMovePower+3; // Sets min power for drive motors
	int wheelRNormal = minMovePower+3;
	int leftTError = 0; int rightTError = 0; int leftTOffset; int rightTOffset; int leftPowertmp; int rightPowertmp; float kp = 0.26;
  while (abs(ticks - SensorValue[LeftEncoder]) > driveThreshold && abs(-ticks - SensorValue[RightEncoder]) > driveThreshold) {
    driveState = 2;
    bob += 1; // Debug
    leftTError = ticks - SensorValue[LeftEncoder];// Determines current "error" (distance from target)
    rightTError = -ticks - SensorValue[RightEncoder];
    leftTOffset = leftTError * kp; // Changes motor power based on distance and speed
    rightTOffset = rightTError * kp; // ie. Farther - faster, Closer - slower
    if (abs(prevEncL - SensorValue[LeftEncoder]) == 0) { // If motor does not turn, increase power
      wheelLNormal += 1;
    }
    if (abs(prevEncR - SensorValue[RightEncoder]) == 0) {
      wheelRNormal += 1;
    }
    //--- if Offset (calculated motor power) is lower than user-set minimum, use minimum.
    if (abs(leftTOffset) < wheelLNormal) {
      leftPowertmp = (leftTOffset < 0)? -wheelLNormal: wheelLNormal;
    }
    else if (abs(leftTOffset) > maxMovePower) {
      leftPowertmp = (leftTOffset < 0)? -maxMovePower: maxMovePower;
    }
    else {
      leftPowertmp = leftTOffset;
    }
    if (abs(rightTOffset) < wheelRNormal) {
      rightPowertmp = (rightTOffset < 0)? -wheelRNormal: wheelRNormal;
    }
    else if (abs(rightTOffset) > maxMovePower) {
      rightPowertmp = (rightTOffset < 0)? -maxMovePower: maxMovePower;
    }
    else {
      rightPowertmp = rightTOffset;
    }
    wheelLeftPower = leftPowertmp;
    wheelRightPower = rightPowertmp;
    prevEncL = SensorValue[LeftEncoder]; // Records encoder readings
    prevEncR = SensorValue[RightEncoder];
    wait1Msec(10);
  }
  wheelLeftPower = (ticks > 0) ? -wheelBrakePower:wheelBrakePower; // Once done, apply "brake"
  wheelRightPower = (ticks > 0) ? wheelBrakePower:-wheelBrakePower;
  driveState = 3;
  wait1Msec(20);
}

void turnT(float ticks) // Turns robot about centre depending on ticks
{
  int prevEncL; int prevEncR;
	SensorValue[LeftEncoder] = 0;
	SensorValue[RightEncoder] = 0;
	int wheelLNormal = 35;
	int wheelRNormal = 35;
  while (abs(ticks - SensorValue[LeftEncoder]) > driveThreshold && abs(-ticks - SensorValue[RightEncoder]) > driveThreshold) {
    driveState = 2;
    bob += 1; // Debug
    if (abs(prevEncL - SensorValue[LeftEncoder]) == 0) { // If motor does not turn, increase min power
      wheelLNormal += 1;
    }
    if (abs(prevEncR - SensorValue[RightEncoder]) == 0) {
      wheelRNormal += 1;
    }
    if ((ticks - SensorValue[LeftEncoder]) < 0) {
      wheelLeftPower = -wheelLNormal;
    }
    else {
      wheelLeftPower = wheelLNormal;
    }
    if ((-ticks - SensorValue[RightEncoder]) < 0) {
      wheelRightPower = -wheelRNormal;
    }
    else {
      wheelRightPower = wheelRNormal;
    }
    prevEncL = SensorValue[LeftEncoder];
    prevEncR = SensorValue[RightEncoder];
    wait1Msec(10);
  }
  wheelLeftPower = 0;
  wheelRightPower = 0;
  driveState = 3;
}

void timedTurn(int speed, int time) { // Turn by time
  wheelLeftPower = speed;
  wheelRightPower = -speed;
  wait10Msec(time);
  wheelLeftPower = 0;
  wheelRightPower = 0;
}

// ---------------------------- Forward -------------------------------------------
void moveForwardTP(float distance, float konstant = 0.28) { // Moves Forward By Ticks with P controller AND D controller
  int prevEncL; int prevEncR;
  int moveamount = distance;
	SensorValue[LeftEncoder] = 0;
	SensorValue[RightEncoder] = 0;
	int wheelLNormal = minMovePower+1; // Sets min power for drive motors
	int wheelRNormal = minMovePower+1;
	int leftMError = 0; int rightMError = 0; int leftMOffset; int rightMOffset; int leftPowertmp; int rightPowertmp; float kp = konstant; float kd = 3.5;
	int leftMprev = (moveamount - SensorValue[LeftEncoder]); int rightMprev = (moveamount - SensorValue[RightEncoder]); int dleftMerror = 0; int drightMerror = 0;
  while (abs(moveamount - SensorValue[LeftEncoder]) > driveThreshold && abs(moveamount - SensorValue[RightEncoder]) > driveThreshold) {
    driveState = 1;
    bob += 1; // Debug
    leftMError = moveamount - SensorValue[LeftEncoder]; // Determines current "error" (distance from target)
    rightMError = moveamount - SensorValue[RightEncoder];
    dleftMerror = leftMError - leftMprev; // Determines rate of change of error (speed of robot)
    drightMerror = rightMError - rightMprev;
    leftMOffset = leftMError * kp + dleftMerror *kd; // Changes motor power based on distance and speed
    rightMOffset = rightMError * kp + drightMerror *kd; // ie. Farther - faster, Closer - slower | Faster - slower, Slower - faster
    if (abs(prevEncL - SensorValue[LeftEncoder]) == 0) { // If motor does not turn, increase min power
      wheelLNormal += 1;
    }
    if (abs(prevEncR - SensorValue[RightEncoder]) == 0) {
      wheelRNormal += 1;
    }
    //--- if Offset (calculated motor power) is lower than user-set minimum, use minimum. If greater than user-set maximum, use maximum
    if (abs(leftMOffset) < wheelLNormal) {
      leftPowertmp = (leftMOffset < 0)? -wheelLNormal: wheelLNormal;
    }
    else if (abs(leftMOffset) > maxMovePower) {
      leftPowertmp = (leftMOffset < 0)? -maxMovePower: maxMovePower;
    }
    else {
      leftPowertmp = leftMOffset;
    }
    if (abs(rightMOffset) < wheelRNormal) {
      rightPowertmp = (rightMOffset < 0)? -wheelRNormal: wheelRNormal;
    }
    else if (abs(rightMOffset) > maxMovePower) {
      rightPowertmp = (rightMOffset < 0)? -maxMovePower: maxMovePower;
    }
    else {
      rightPowertmp = rightMOffset;
    }
    //--
    wheelLeftPower = leftPowertmp;
    wheelRightPower = rightPowertmp;
    leftMprev = leftMError; // records the current error
    rightMprev = rightMError;
    prevEncL = SensorValue[LeftEncoder];
    prevEncR = SensorValue[RightEncoder];
    wait1Msec(10);
  }
  wheelLeftPower = wheelRightPower = (distance > 0) ? -wheelBrakePower:wheelBrakePower; // Once done, apply "brake"
  driveState = 3;
  wait1Msec(20);
}

void timedMove(int speed, int time) { // Move Forward by time
  wheelLeftPower = speed;
  wheelRightPower = speed;
  wait10Msec(time);
  wheelLeftPower = 0;
  wheelRightPower = 0;
}

// ----------------------------- Ultrasonic -----------------
void ultraLowMove(int ticks, int power = 5, int time = 240, int ultraVal = 65) { // Moves robot until a specific ultrasonic value is reached.
  int timer = 0;
	while ((SensorValue[Ultrasonic] > ultraVal || SensorValue[Ultrasonic] == -1) && timer < time) { // While ultrasonic value is greater than desired and time is less than time limit...
    driveState = 4;
	  wheelLeftPower = minMovePower + power; //... move forward
    wheelRightPower = minMovePower + power;
    wait1Msec(10);
    timer += 1;
	}
	wheelLeftPower = -wheelBrakePower;
  wheelRightPower = -wheelBrakePower;
  driveState = 3;
  wait10Msec(30);
  moveForwardTP(ticks);
}

// ------------------------------------ Other -----------------------------------

void correctWheels (int time) { // If robot is tilted to one side, this functions straightens the robot
  int timer = 0;
  if (SensorValue[LeftEncoder] > SensorValue[RightEncoder]) {
	  while (SensorValue[LeftEncoder] > SensorValue[RightEncoder] && timer < time) {
	  wheelLeftPower = -(minMovePower + 17);
	  wheelRightPower = minMovePower + 17;
	  wait1Msec(10);
	  timer += 1;
	  }
	}
	else if (SensorValue[LeftEncoder] < SensorValue[RightEncoder]) {
	  while (SensorValue[LeftEncoder] < SensorValue[RightEncoder] && timer < time) {
	    wheelLeftPower = (minMovePower + 10);
	    wheelRightPower = -(minMovePower + 10);
	    wait1Msec(10);
	    timer += 1;
	  }
  }
  wheelLeftPower = 0;
  wheelRightPower = 0;
}

void wiggle() { // JIGGLE!!! Moves robot forward and back, forward and back
  if (wiggledir > 0) { // Moves forward for 130ms
    wheelLeftPower = minMovePower + 22;
    wheelRightPower = minMovePower + 22;
    wait1Msec(130);
    wiggledir *= -1; // Switch direction
  }
  else if (wiggledir < 0) { // Moves backward for 130ms
    wheelLeftPower = -(minMovePower + 22);
    wheelRightPower = -(minMovePower + 22);
    wait1Msec(130);
    wiggledir *= -1; // Switch direction
  }
}
